
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_qmi8658.c
  * @author     baiyang
  * @date       2022-8-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu_qmi8658.h"
#include "sensor_imu_qmi8658_registers.h"

#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/utility/sparse-endian.h>
/*-----------------------------------macro------------------------------------*/
#define QMI8658_BACKEND_SAMPLE_RATE   1000
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_imu_qmi8658_start(sensor_imu_backend *backend);
static bool sensor_imu_qmi8658_update(sensor_imu_backend *backend);
static bool init(sensor_imu_qmi8658_t qmi8658);
static bool hardware_init(sensor_imu_qmi8658_t qmi8658);
static bool check_whoami(sensor_imu_qmi8658_t qmi8658);
static void fifo_reset(sensor_imu_qmi8658_t qmi8658);
static void read_sample(void *parameter);
static inline uint8_t _register_read(sensor_imu_qmi8658_t qmi8658, uint8_t reg);
static inline bool _block_read(sensor_imu_qmi8658_t qmi8658, uint8_t reg, uint8_t *buf, uint32_t size);
static inline bool _register_write(sensor_imu_qmi8658_t qmi8658, uint8_t reg, uint8_t val);
/*----------------------------------variable----------------------------------*/
static struct sensor_imu_backend_ops qmi8658_ops = {.sensor_imu_backend_destructor = NULL,
                                                    .update = sensor_imu_qmi8658_update,
                                                    .accumulate = NULL,
                                                    .start = sensor_imu_qmi8658_start,
                                                    .get_output_banner = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_qmi8658_ctor(sensor_imu_qmi8658_t qmi8658,
                                    gp_device_t dev,
                                    enum RotationEnum rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(qmi8658, 0, sizeof(struct sensor_imu_qmi8658));

    sensor_imu_backend_ctor(&qmi8658->backend, &qmi8658_ops, "qmi8658");

    qmi8658->_rotation = rotation;
    
    qmi8658->_dev = dev;
}

sensor_imu_backend * sensor_imu_qmi8658_probe(gp_device_t dev, enum RotationEnum rotation)
{
    if (!dev) {
        return NULL;
    }

    sensor_imu_qmi8658_t sensor = (sensor_imu_qmi8658_t)rt_malloc(sizeof(struct sensor_imu_qmi8658));

    if (!sensor) {
        return NULL;
    }

    sensor_imu_qmi8658_ctor(sensor, dev, rotation);

    if (!init(sensor)) {
        sensor_imu_backend_destructor(&sensor->backend);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_imu_backend *)sensor;
}

static void sensor_imu_qmi8658_start(sensor_imu_backend *backend)
{
    sensor_imu_qmi8658_t qmi8658 = (sensor_imu_qmi8658_t)backend;

    fifo_reset(qmi8658);

    if (!sensor_imu_register_accel(&qmi8658->_accel_instance, QMI8658_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(qmi8658->_dev, DEVTYPE_INS_QMI8658)) ||
        !sensor_imu_register_gyro(&qmi8658->_gyro_instance, QMI8658_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(qmi8658->_dev, DEVTYPE_INS_QMI8658))) {
        return;
    }

    // setup sensor rotations from probe()
    sensor_imu_backend_set_gyro_orientation(qmi8658->_gyro_instance, qmi8658->_rotation);
    sensor_imu_backend_set_accel_orientation(qmi8658->_accel_instance, qmi8658->_rotation);
    
    // setup callbacks
    devmgr_register_periodic_callback(qmi8658->_dev, 1000000UL / QMI8658_BACKEND_SAMPLE_RATE, read_sample, qmi8658);
}

static bool sensor_imu_qmi8658_update(sensor_imu_backend *backend)
{
    sensor_imu_qmi8658_t qmi8658 = (sensor_imu_qmi8658_t)backend;

    sensor_imu_backend_update_accel(backend, qmi8658->_accel_instance);
    sensor_imu_backend_update_gyro(backend, qmi8658->_gyro_instance);
    return true;
}

static bool init(sensor_imu_qmi8658_t qmi8658)
{
    devmgr_set_read_flag(qmi8658->_dev, 0x80);

    return hardware_init(qmi8658);
}

static bool hardware_init(sensor_imu_qmi8658_t qmi8658)
{
    devmgr_take_bus(qmi8658->_dev);

    rt_thread_mdelay(10);

    // initially run the bus at low speed
    devmgr_set_speed(qmi8658->_dev, DEV_SPEED_LOW);

    if (!check_whoami(qmi8658)) {
        console_printf("[QMI8658]: _check_whoami fail\n");
        goto failed;
    }

    //四线SPI，串行接口（SPI 或 I2C）地址自动递增, 大端读取，使能内部2M晶振
    if (!_register_write(qmi8658, QMI8658REGISTER_CTRL1, 0x60)) {
        goto failed;
    }

    //加速度计自检，±16g，2000Hz采样
    if (!_register_write(qmi8658, QMI8658REGISTER_CTRL2, QMI8658ACCRANGE_16G | QMI8658ACCODR_2000HZ)) {
        goto failed;
    }

    qmi8658->_accel_scale = GRAVITY_MSS * 16.0f /32768.0f;

    //陀螺仪自检，±2048dps，2000Hz采样
    if (!_register_write(qmi8658, QMI8658REGISTER_CTRL3, QMI8658GYRRANGE_2048DPS | QMI8658GYRODR_2000HZ)) {
        goto failed;
    }

    qmi8658->_gyro_scale = radians(2048.0f)/32768.0f;

    //无磁力仪
    if (!_register_write(qmi8658, QMI8658REGISTER_CTRL4, 0x00)) {
        goto failed;
    }

    //使能陀螺仪低通滤波, 14.0%采样频率
    if (!_register_write(qmi8658, QMI8658REGISTER_CTRL5, (0x03 << 5) | (0x01 << 4 ) | (0x03 << 1 ) | (0x01 << 0))) {
        goto failed;
    }

    //使能陀螺仪、加速度计
    if (!_register_write(qmi8658, QMI8658REGISTER_CTRL7, 0x03)) {
        goto failed;
    }

    devmgr_set_speed(qmi8658->_dev, DEV_SPEED_HIGH);
    devmgr_set_device_type(qmi8658->_dev, DEVTYPE_INS_QMI8658);

    console_printf("QMI8658 found on bus 0x%x\n", (unsigned)devmgr_get_bus_id(qmi8658->_dev));
    devmgr_release_bus(qmi8658->_dev);

    rt_thread_mdelay(10);

    return true;

failed:
    devmgr_release_bus(qmi8658->_dev);
    return false;
}

/*
  check whoami for sensor type
 */
static bool check_whoami(sensor_imu_qmi8658_t qmi8658)
{
    uint8_t whoami = _register_read(qmi8658, QMI8658REGISTER_WHOAMI);
    uint8_t revision_id = _register_read(qmi8658, QMI8658REGISTER_REVISION);

    if (whoami == QMI8658_WHOAMI_VAL) {
        return true;
    }

    // not a value WHOAMI result
    return false;
}

static void fifo_reset(sensor_imu_qmi8658_t qmi8658)
{
    // 禁用fifo
    // TOOD: 使能fifo,64 samples,Stream
    _register_write(qmi8658, QMI8658REGISTER_FIFOCTRL, 0x00);

    sensor_imu_backend_notify_accel_fifo_reset(qmi8658->_accel_instance);
    sensor_imu_backend_notify_gyro_fifo_reset(qmi8658->_gyro_instance);
}

/*
  read qmi8658 fifo
 */
static void read_sample(void *parameter)
{
    sensor_imu_qmi8658_t qmi8658 = (sensor_imu_qmi8658_t)parameter;

    uint8_t status0;
    if (!devmgr_read_registers(qmi8658->_dev, QMI8658REGISTER_STATUS0, &status0, 1)) {
        sensor_imu_backend_inc_accel_error_count(qmi8658->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(qmi8658->_gyro_instance);
        return;
    }

    // 检查加速度计、陀螺仪是否有新数据
    if (!(status0 & QMI8658STATUS0_ADA) || !(status0 & QMI8658STATUS0_GDA)) {
        return;
    }

    struct qmi8658_fifo_accgyr accgyr;
    if (!devmgr_read_registers(qmi8658->_dev, QMI8658REGISTER_AX_L, (uint8_t *)&accgyr, sizeof(struct qmi8658_fifo_accgyr))) {
        sensor_imu_backend_inc_accel_error_count(qmi8658->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(qmi8658->_gyro_instance);
        return;
    }

    Vector3f_t accel = {(int16_t)(accgyr.accel[0]) * qmi8658->_accel_scale,
                        (int16_t)(accgyr.accel[1]) * qmi8658->_accel_scale,
                        (int16_t)(accgyr.accel[2]) * qmi8658->_accel_scale};

    Vector3f_t gyro = {(int16_t)(accgyr.gyro[0]) * qmi8658->_gyro_scale,
                       (int16_t)(accgyr.gyro[1]) * qmi8658->_gyro_scale,
                       (int16_t)(accgyr.gyro[2]) * qmi8658->_gyro_scale};

    sensor_imu_backend_rotate_and_correct_accel(&qmi8658->backend, qmi8658->_accel_instance, &accel);
    sensor_imu_backend_notify_new_accel_raw_sample(&qmi8658->backend, qmi8658->_accel_instance, &accel, 0, false);

    sensor_imu_backend_rotate_and_correct_gyro(&qmi8658->backend, qmi8658->_gyro_instance, &gyro);
    sensor_imu_backend_notify_new_gyro_raw_sample(&qmi8658->backend, qmi8658->_gyro_instance, &gyro, 0);

    if (qmi8658->_temperature_counter++ == 100) {
        qmi8658->_temperature_counter = 0;
        int16_t t;
        if (!devmgr_read_registers(qmi8658->_dev, QMI8658REGISTER_TEMPEARTURE_L, (uint8_t *)&t, sizeof(t))) {
            sensor_imu_backend_inc_accel_error_count(qmi8658->_accel_instance);
            sensor_imu_backend_inc_gyro_error_count(qmi8658->_gyro_instance);
        } else {
            float temp_degc = 0.00390625 * t;
            sensor_imu_backend_publish_temperature(&qmi8658->backend, qmi8658->_accel_instance, temp_degc);
        }
    }

    struct checkreg reg;
    if (!devmgr_check_next_register2(qmi8658->_dev, &reg)) {
        //log_register_change(dev_accel->get_bus_id(), reg);
        sensor_imu_backend_inc_accel_error_count(qmi8658->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(qmi8658->_gyro_instance);
    }
}

static inline uint8_t _register_read(sensor_imu_qmi8658_t qmi8658, uint8_t reg)
{
    uint8_t val = 0;
    devmgr_read_registers(qmi8658->_dev, reg, &val, 1);

    return val;
}

static inline bool _block_read(sensor_imu_qmi8658_t qmi8658, uint8_t reg, uint8_t *buf, uint32_t size)
{
    return devmgr_read_registers(qmi8658->_dev, reg, buf, size);
}

static inline bool _register_write(sensor_imu_qmi8658_t qmi8658, uint8_t reg, uint8_t val)
{
    return devmgr_write_register(qmi8658->_dev, reg, val, false);
}

/*------------------------------------test------------------------------------*/


